Omnidirectional wheeled humanoid robot based on a linear predictive position and velocity controller

ABSTRACT

A humanoid robot with a body joined to an omnidirectional mobile ground base, equipped with: a body position sensor, a base position sensor and an angular velocity sensor to provide measures, actuators comprising at least 3 wheels located in the omnidirectional mobile base, extractors for converting sensored measures into useful data, a supervisor to calculate position, velocity and acceleration commands from the useful data, means for converting commands into instructions for the actuators, wherein the supervisor comprises: a no-tilt state controller, a tilt state controller and a landing state controller, each controller comprising means for calculating, position, velocity and acceleration commands based on a double point-mass robot model with tilt motion and on a linear model predictive control law, expressed as a quadratic optimization formulation with a weighted sum of objectives, and a set of predefined linear constraints.

The present invention relates to the field of robot programming systems.More specifically, it applies to the controlling of motions of robotswhich move around on articulated limbs or use them, notably robots ofhuman or animal form. A robot can be qualified as humanoid from themoment when it has certain human appearance attributes: a head, a trunk,two arms, two hands, etc. A humanoid robot may, however, be more or lesssophisticated. Its limbs may have a greater or lesser number ofarticulations. It may control its own balance statically and dynamicallyand roll over a base. It may pick up signals from the environment(“hear”, “see”, “touch”, “sense”, etc.) and react according to more orless sophisticated behaviors, and interact with other robots or humans,either by speech or by gesture. For a current generation of humanoidrobots, programmers are capable of creating scenarios, which can be moreor less sophisticated, as sequences of events/actions reactedto/performed by the robot. These actions can be conditional upon certainbehaviors of people who interact with the robot. But in these humanoidrobots of the first generation, application programming is done in adevelopment toolkit and each application needs to be launched by atriggering event, the occurrence of which has been included in theapplication.

There is therefore a need for a humanoid robot capable of living an“autonomous life”, as a human being does, who is capable of behaving ina determined manner, depending on the environment he is evolving in. Itis an object of the present invention to overcome the limitations of therobots of the prior art by providing a robot which is capable ofdetermining autonomously sequences of its life adapted to the context itis evolving in, without any intervention of a programmer.

We consider a robot with a mobile base joined to a body, also calledupper body.

In standard behavior, the robot is in contact with all of its wheels,and dynamically stable while the Center of Pressure (CoP) which is thebarycenter of the forces acting between the ground and the robot, isstrictly inside the convex hull defined by its wheel contact points. Inabsence of disturbance, this CoP constraint is always respected.However, when disturbances appear, the CoP can be on the bounds of thesupport hull. In that case, the robot can start to tilt, and if nothingis done, it can fall. So our problem is to control the robot over twodynamic models: when the robot is not tilting, and when it is.

When the robot is not tilting, we can found some papers aboutcontrolling a mobile robot with dynamic stability constraints, or aboutcontrolling a humanoid two-legged robots.

Some recent works deal with controlling a robot with dynamicalconstraints, caused by limbs such as a manipulator arm. K. Mingeuk andal. have worked on the stabilization of a wheeled platform using dynamicconstraints: “Stabilization of a rapid four-wheeled mobile platformusing the zmp stabilization method”. They use a direct linear quadraticregulator (LQR) method to control the platform. The inconvenient of thismethod is that the submitted dynamic constraints require having the CoP(Center of Pressure) on the middle of the platform. The CoP is thebarycenter of the contact forces between the robot and the ground. Thismethod involves losing several DoF (Degree of Freedom): indeed, toprevent a robot from falling, the CoP needs to be only in the convexpolygon defined by the contact points between the wheels and the ground.

In another paper, Y. Li and al. present a simple controller of a mobilerobot with dynamic constraints: “The dynamic stability criterion on thewheel-based humanoid robot based on zmp modeling”. The difference withthe K. Mingeuk and A I. publication is that it takes into account thefull CoP constraint, which is a sum of inequality constraints. Thiscontroller is a pid-control iterated on a complete model of the robot tofind a torque command where the CoP is in the support polygon.

Concerning humanoid robotics, P. B. Wieber, H. Diedam and A. Herdt havedescribed a method to control humanoid two-legged robot with highlyconstrained dynamic: “Walking without thinking about it”. This mostrecent approach concerns the linear predictive control based on a 3dlinear inverted pendulum model. Using a simple model of the robot, thiscontrol law predicts the dynamic of its state in the future, in order toensure that the current command sent to the robot will not cause aninevitable fall in a few seconds. Concerning the biped humanoid robotNAO, an implementation of this control law can be found in a paperwritten by D. Gouaillier, C. Collette and K. Kilner: “Omni-directionalclosed-loop walk for NAO”. But the robot NAO is small and this methodwould not give good results notably for a higher humanoid robot as shownFIG. 1, for instance with the following features:

-   -   20 Degrees of freedom (DoF) (2 DoF on the head 160, 2×6 DoF on        the arms 170, 3 DoF on the leg 180 and 3 DoF in the mobile base        140),    -   1.37 m height 110,    -   0.65 m width 130,    -   0.40 m depth 120,    -   30 kg total mass,    -   one leg 180 linked to the omnidirectional base 140 with three        wheels 141. The mobile base has a triangular shape of 0.422 m        length and is able to move the robot at a maximum velocity of        1:4 m/s⁻¹ and acceleration of 1:7 m/s⁻² for a short time. The        nominal velocity and acceleration are 0:5 m/s⁻¹ and 1:0 m/s⁻².

Concerning the case when the robot is tilting, we can link this problemwith the control of one or two wheeled mobile robot. However, in ourcase, we do not want to control the robot around an unstable equilibriumdirection (generally the vertical), like all one or two wheeled mobilerobots. Many papers can be found about controlling these robots. Theyare based on different types of control like:

PID force control as described in the publication by J. K. Ahn and S. J.Lee, <<Force control application to a mobile manipulator with balancingmechanism,” 2010,

Linear Quadratic Regulator as described in the publication by X.Changkai, L. Ming, and P. Fangyu, “The system design and lqr control ofa two-wheels self-balancing mobile robot,” 2011.or

Sliding Mode Control as described in the publication by K. Sho-Tsung, C.Wan-Jung, and H. Ming-Tzu, “Balancing of a spherical inverted pendulumwith an omni-directional mobile robot,” IEEE Multi-conference on Systemsand Control, 2013.

All of these controllers have for objective to control the robot aroundan unstable equilibrium direction, depending on gravity and on externalforces.

Concerning humanoid robotic, we can found some papers about pushrecovering. The main concepts are to manage over four possibilities ofcontrol:

-   -   move the CoP using the upper bodies,    -   create a torque using the ankle of the foot,    -   create some angular momentums using the arms, and    -   do some steps.

They cannot be applied in our case because they strongly depend on thebipedal mechanic, which implies that the feet cannot roll or slip on theground, and that they can do some step to recover from a push. In ourcase, the CoP cannot be displaced because there is no surface of contacton the tilt direction. Also another difficulty of our problem is to takeinto account the discrete switch of the dynamic model (presence orabsence of some contact forces). This implies that some impact canoccur. Generally, manage with impact is a hard question for real-timecontrol.

There is therefore a need for controlling both the mobile base of ahumanoid robot and its body, while taking into account their dynamicalconstraints and that the robot can be strongly disturbed and can tiltaround its wheels.

So, instead of proposing a unique controller which will be valid in thedifferent possible dynamical states, we define two controllers, one whenthe robot is not tilting, and one when it is. In order to ensure acorrect and smooth transition between the two controllers, a thirdcontroller is defined to deal with a landing phase and a supervisor isdefined to address them accordingly.

To this effect, the invention provides a humanoid robot with a bodyjoined to an omnidirectional mobile ground base, equipped with:

a body position sensor, a base position sensor and an angular velocitysensor to provide measures,

actuators comprising joints motors and at least 3 wheels located in theomnidirectional mobile base,

extractors for converting sensored measures into useful data,

a supervisor to calculate position, velocity and acceleration commandsfrom the useful data,

means for converting commands into instructions for the actuators.

It is mainly characterized in that the supervisor comprises:

a no-tilt state controller, a tilt state controller and a landing statecontroller, each controller comprising means for calculating from theuseful data, pre-ordered position and velocity references, and a tiltangle and angular velocity references set to 0, position, velocity andacceleration commands based on a double point-mass robot model with tiltmotion and on a linear model predictive control law with a discretizedtime according to a sampling time period T and a number N of predictedsamples, expressed as a quadratic optimization formulation with aweighted sum of objectives, and a set of predefined linear constraints,

an impact angular velocity and a landing impact time estimator and

means for choosing a controller according to an estimated impact angularvelocity and useful angular velocity.

The advantage of this robot is to be robust to many types ofdisturbances. They can be short, long, fast or multiple, with greatdynamic bandwidth. We make no hypothesis about the disturbances.Moreover, in case of a very strong disturbance which cannot be recoveredfrom, a fall manager strategy has been implemented on the robot, tominimize the impact of the body with the ground.

More precisely, the advantages of this robot are firstly to be able torecover from a strong disturbance which makes the robot tilt. The secondadvantage is to control a non-linear dynamic, with discrete change andimpact in a simple and real-time way. The third advantage is to be ableto define many behaviors of the robot by tuning the parameters of thesupervisor as choosing to more limit the mechanical impact, or to limitthe travelled distance during the tilt recovery.

According to an embodiment of the invention, the estimated impactangular velocity is calculated by integration of a constant.

There can be no tilt in the robot model ; the no-tilt controller can beable for instance to calculate position, velocity and accelerationcommands from the useful data using pre-ordered references, with thefollowing objectives:

a base position objective,

a base velocity objective,

an objective related to the distance between the CoP and the basecenter, CoP being the barycenter of contact forces between the robot andthe ground,

and with the following constraints:

-   -   a maximum velocity and acceleration of the mobile base,    -   kinematic limits of the body,    -   a CoP limit.

The tilt controller can be able to calculate position, velocity andacceleration commands from the useful data using pre-ordered referencesand a tilt angle and angular velocity references set to 0, and withobjectives that are tilt angle minimization and angular velocityminimization and with constraints that are kinematic limits of themobile base, kinematic limits of the body, a positive tilt angle and amove of the body only on the angular velocity axis.

A weighted numerical stability objective can be added to the weightedsum of objectives.

The landing controller can be able to calculate position, velocity andacceleration commands from the useful data using pre-ordered referencesand a tilt angle and angular velocity references set to 0, withobjectives that are an objective related to the distance between the CoPand the base center, CoP being the barycenter of contact forces betweenthe robot and the ground, and a numerical stability objective, and withconstraints that are a maximum velocity and acceleration of the mobilebase and kinematics limits of the body and a CoP limit and a move of thebody only on the angular velocity axis.

A base velocity objective can be added to the weighted sum of theseobjectives.

The kinematics limits of the body can be null.

According to an embodiment of the invention, at least a wheel isomnidirectional.

Advantageously, the ground is planar and horizontal.

The invention also provides a method for controlling a humanoid robotwith a body joined to an omnidirectional mobile ground base, implementedat pre-defined sampling times and comprising:

retrieving position measure of the body, position measure of the base,tilt angle of the robot and angular velocity measure of the robot,

converting these measures in useful data,

using the useful data, and according to a state of the robot,calculating position, velocity and acceleration commands using a controllaw based on a linear model predictive control law with a discretizedtime according to a sampling time period and a number of predictedsamples, and expressed as a quadratic optimization formulation with aweighted sum of objectives with predefined weights and a set of linearconstraints,

converting these commands into instructions for the robot actuators.

The state of the robot can be defined according to the following steps:

-   -   if one among the tilt angle measure or/and the angular velocity        measure is greater than zero, estimate an estimated impact        angular velocity and an estimated impact time,    -   initially, the robot is in a no-tilt state,    -   No-tilt state:        -   If there is no estimated impact time, switch to a tilt            state;        -   If the impact angular velocity is above a pre-defined            limit1, switch to the tilt state;        -   If the measured angular velocity is above a pre-defined            limit2, switch to the tilt state;        -   Else, if the tilt angle is not null, switch to a landing            state,            -   if the tilt angle is null stay in the no-tilt state.    -   Tilt state:        -   If the measured angular velocity is above the pre-defined            limit2, stay in tilt state.        -   If the estimated impact velocity is under a pre-defined            limit1, switch to the landing state        -   Else, stay in tilt state.    -   Landing state:        -   If there is no estimated impact time, switch to the tilt            state;        -   If the impact angular velocity is above a pre-defined            limit1, switch to the tilt state;        -   If the measured angular velocity is above a pre-defined            limit2, switch to the tilt state;        -   If the tilt angle falls to 0, and if an angular velocity            measured at a next time is null, switch to the non-tilt            state, else, stay in the landing state.

The invention also provides a computer program comprising computer codefit for executing when running on a computer the method as described.

The invention will be better understood and its various features andadvantages will emerge from the following description of a number ofexemplary embodiments and its appended figures in which:

FIG. 1 displays a physical architecture of a humanoid robot in a numberof embodiments of the invention;

FIG. 2 displays a functional architecture of the software modules of ahumanoid robot in a number of embodiments of the invention;

FIG. 3 displays a modelization of the mass repartition of a robotdesigned according to the invention,

FIG. 4 displays a modelization of the tilt dynamic of a robot accordingto the invention,

FIG. 5 illustrates a geometrical representation of a constraint,

FIG. 6 schematically represents a functional scheme of a supervisor.

From a figure to another one, the same elements are tagged with the samereferences.

FIG. 1 displays a physical architecture of a humanoid robot in a numberof embodiments of the invention.

The specific robot 100 in the figure is taken as an example only of ahumanoid robot in which the invention can be implemented. The lower limb180 of the robot in the figure is not functional for walking, but canmove in any direction on its base 140 which rolls on the surface onwhich it lays. By way of example, this robot has a height 110 which canbe around 120 cm, a depth 120 around 65 cm and a width 130 around 40 cm.In a specific embodiment, the robot of the invention has a tablet 150with which it can communicate messages (audio, video, web pages) to itsenvironment, or receive entries from users through the tactile interfaceof the tablet. In addition to the processor of the tablet, the robot ofthe invention also uses the processor of its own motherboard, which canfor example be an ATOM™ Z530 from Intel™. The robot of the inventionalso advantageously includes a processor which is dedicated to thehandling of the data flows between the motherboard and actuators whichcontrol the motors of the joints in a limb and the balls that the robotuses as wheels, in a specific embodiment of the invention. The motorscan be of different types, depending on the magnitude of the maximumtorque which is needed for a definite joint. For instance, brush DCcoreless motors from e-minebea™ (SE24P2CTCA for instance) can be used,or brushless DC motors from Maxon™ (EC45_70W for instance). The MREs arepreferably of a type using the Hall effect, with 12 or 14 bitsprecision.

In embodiments of the invention, the robot displayed in FIG. 1 alsocomprises various kinds of sensors. Some of them are used to control theposition and movements of the robot. This is the case, for instance, ofan inertial unit, located in the torso of the robot, comprising a 3-axesgyrometer and a 3-axes accelerometer. The robot can also include two 2Dcolor RGB cameras on the forehead of the robot (top and bottom) of theSystem On Chip (SOC) type, such as those from Shenzen V-VisionTechnology Ltd™ (OV5640), with a 5 megapixels resolution at 5 frames persecond and a field of view (FOV) of about 57° horizontal and 44°vertical. One 3D sensor can also be included behind the eyes of therobot, such as an ASUS XTION™ SOC sensor with a resolution of 0.3megapixels at 20 frames per second, with about the same FOV as the 2Dcameras. The robot of the invention can also be equipped with laserlines generators, for instance three in the head and three in the base,so as to be able to sense its relative position to objects/beings in itsenvironment. The robot of the invention can also include microphones tobe capable of sensing sounds in its environment. In an embodiment, fourmicrophones with a sensitivity of 300 mV/Pa +/−3 dB at 1 kHz and afrequency range of 300 Hz to 12 kHz (−10 dB relative to 1 kHz) can beimplanted on the head of the robot. The robot of the invention can alsoinclude two sonar sensors, possibly located at the front and the back ofits base, to measure the distance to objects/human beings in itsenvironment. The robot can also include tactile sensors, on its head andon its hands, to allow interaction with human beings.

To translate its emotions and communicate with human beings in itsenvironment, the robot of the invention can also include:

LEDs, for instance in its eyes, ears and on its shoulders;

Loudspeakers, for instance two, located in its ears.

The robot of the invention may communicate with a base station or otherrobots through an Ethernet RJ45 or a WiFi 802.11 connection.

The robot of the invention can be powered by a Lithium Iron Phosphatebattery with energy of about 400 Wh. The robot can access a chargingstation fit for the type of battery included.

Position/movements of the robot are controlled by its motors, usingalgorithms which activate the chains defined by each limb and effectorsdefined at the end of each limb, in view of the measurements of thesensors.

FIG. 2 displays a functional architecture of the software modules of ahumanoid robot in a number of embodiments of the invention.

The goal of the invention is to provide a method to allow a humanoidrobot to perform activities in an autonomous way, without anyintervention of a programmer to anticipate the conditions that the robotwill face. In the prior art, the robot is capable of executing scenarioswhich have been programmed and uploaded to its motherboard. Thesescenarios can include reactions to conditions which vary in itsenvironment, but the robot will not be capable to react to conditionswhich have not been predicted and included in the code uploaded to itsmotherboard or accessed distantly. In contrast, the goal of theinvention is to allow the robot to behave autonomously, even in view ofevents/conditions which have not been predicted by a programmer. Thisgoal is achieved by the functional architecture which is displayed inFIG. 2.

This functional architecture comprises in essence four main softwaremodules.

A Services module 210 includes services of at least three types:

Extractor Services 211, which receives as input readings from the robotsensors of the type described in relation with FIG. 1; these sensorreadings are preprocessed so as to extract relevant data (also saiduseful data) in relation to the position of the robot, identification ofobjects/human beings in its environment, distance of said objects/humanbeings, words pronounced by human beings or emotions thereof; example ofExtractor Services are: People Perception to perceive the presence ofhuman beings in the vicinity of the robot, Movement Detection to detectthe movements of these human beings; Sound Localization to locate asound, Touch Detection to interpret a touch on a robot tactile sensor,Speech Recognition, Emotion Recognition, to identify the emotionexpressed by a human being in the vicinity of the robot, through itswords or gestures;

Actuator Services 212, which control physical actions of the robot suchas Motion to activate the motors of the joints or the base, Tracker tofollow motion of a human being in the environment of the robot, lightingof the robot's LEDs to communicate emotions, Animated Speech(combinations of speech and gestures), Behaviors; behaviors are acombination of movements, words, lightings which may express emotions ofthe robot and allow it to perform complex actions;

System Services 213, which notably include Data Services; Data Servicesprovide stored data, either transiently or long-term; examples of DataServices are:

-   -   User Session Service, which stores user data, and their history        of what they have done with the robot;    -   Package Manager Service, which provides a scalable storage of        procedures executed by the robot, with their high level        definition, launching conditions and tags.

An Activities module 220 includes behaviors 221 of the robot which havebeen preprogrammed. The programming of the Behaviors can be effectedusing a graphical integrated development environment (Choregaphe™) whichis the object of a European patent application published under no.EP2435216, which is assigned to the applicant of this patentapplication. Behaviors programmed with Choregaphe™ combine a time basedlogic and an event based logic. Each Behavior is tagged with a Manifest222 which is a text file including notably the launching conditions ofthe Behavior. These launching conditions are based on what theextractors 211 are able to perceive.

A Mind module 230 is in charge of selecting one or several Activities tolaunch. To do so, the Mind subscribes to the Extractors and evaluatesthe launching conditions of all the installed Activities. The variablesof these conditions are event based. So, efficiently, only the conditionstatements containing changed variables need to be reevaluated. Based onthe results of the sorting algorithm, its priorities, and the life cycle(see below), Activities may be launched, and some Activities possiblystopped.

An executed Activity will rely on API (acronym of the French expression“Application Pour Interface”) calls to the Services to perform the taskit has been programmed to do. Whenever an Activity is on stage orstopped, the Mind collects data about when this happened, the currentstate of conditions, and what the user feedback seemed to be tofacilitate learning.

If an existing Extractor event is not sufficient for the conditions,developers can create an additional Extractor to generate a new event,and distribute it in a package with their application.

To do so, the Mind module 230 ties together the Services and theActivities modules by controlling the selection of the Activities, andlaunching the Actuators based on the readings of the Extractors and onalgorithms performed in the Mind called Selectors. Examples of Selectorsare:

Autonomous Life 231, which executes Activities; based on the context ofa situation, the Mind can tell Autonomous Life which activity to focuson (see examples below); any Activity has full access to all callprocedures of the module API; Activities can include a constraint whichwill direct Autonomous Life to focus on a definite Activity;

Basic Awareness 232, which subscribes to Extractor Services such asPeople Perception, Movement Detection, and Sound Localization to tellthe Motion Service to move; the Mind configures Basic Awareness'sbehavior based on the situation; at other times, Basic Awareness iseither acting on its own, or is being configured by a running Activity;

Dialog 233, which subscribes to the Speech Recognition Extractor anduses Animated Speech Actuator Service to speak; based on the context ofa situation, the Mind can tell the Dialog what topics to focus on;metadata in manifests tie this information into the mind; Dialog alsohas its algorithms for managing a conversation and is usually acting onits own.

An Execution Engine 240 launches the API calls to invoke the Services.

The goal of the invention is more specifically to control anomnidirectional wheeled humanoid robot which can be strongly disturbedand tilt around its wheels. The method is based on two linear modelpredictive controllers, depending on the dynamic model of the robot andon a third controller to ensure a correct and smooth transition betweenthe first two controllers. An estimator is designed to detect when therobot enters in a tilt dynamic, or when it returns to its nominal stabledynamic.

Using:

pre-ordered position and velocity references, and a tilt angle andangular velocity references set to 0,

body position, base position and angular velocity data extracted fromsensored measures, and called useful data,

each controller calculates position, velocity and acceleration commandsbased on:

-   -   a double point-mass robot model with tilt motion as shown FIGS.        3 and 4. The first point-mass b represents the mobile base        Center of Mass (CoM), and the second point-mass c represents the        upper body CoM; the joint between the mobile base 140 and the        body (or upper body) 190 is considered as having no mass. When        the robot is not tilting, the CoM of the upper body and of the        mobile base are c_(c) and b_(c). The tilt angle is ψ; h is the        height of c_(c). This model is stored as a system data-service        213.    -   a linear model predictive control law with a discretized time        according to a sampling time period T and a number N of        predicted samples, expressed as a quadratic optimization        formulation with a weighted sum of objectives, and a set of        predefined linear constraints. In order to define the dynamic        behavior of the controlled body and base (c and b), we have to        choose first the duration of the prediction (horizon) and the        period between each sampling instant. Choosing a horizon as        small as possible and a period as large as possible will reduce        the computational time, but will also reduce the stability and        the robustness of the control, depending on the dynamic class.        To conserve the linearity of the system, we have chosen a        polynomial class of order 3 for the body and base trajectories        in order to have a continuous CoP trajectory, in order to avoid        peak of forces in the robot. Also, in this controller, the time        is sampled, with a sampling period T. The number of predicted        sample is N. Another advantage of this kind of control is that        it is simple to manage with many inequalities constraints, as        kinematic limits of the robot, maximum velocity and acceleration        of the mobile base and CoP limits. This control law is stored as        a system data-service 213 which calls for Motion API.

Calculating position, velocity and acceleration commands entailscontrolling the tilt angle and the angular velocity of the robot.

The first controller is dedicated to the situation when the robot is nottilting: then the tilt angle is null.

In order to have a real-time control, the model of the robot needs to beas simple as possible. We need to compute a good approximation of theforces acting by the base on the ground. The repartition of the massesin the robot cannot be done by a single point-mass model, because aroundhalf of the mass is concentrated in the base, and the rest in the upperbody.

We can now write the equations of Newton and Euler for this system,where the axis z is the vertical axe and, x and y the two horizontalaxes:

m _(c)({umlaut over (c)}+g)=F _(b/c)   (1)

m _(b)({umlaut over (b)}+g)=F _(g/b) −F _(b/c)   (2)

{dot over (L)} _(c)=(b−c)×F _(b/c)   (3)

{dot over (L)} _(b)=(p−b)×F _(g/b)   (4)

where mc and mb are the masses respectively linked to c and b, {dot over(L)}_(c)and {dot over (L)}_(b) the angular momentums for eachpoint-mass. F_(b/c) ^(xyz) corresponds to the forces acting by themobile base on the upper body and F_(b/c) ^(xyz) corresponds to theforces acting by the ground on the three wheels of the mobile base.

Also, p is the CoP of the forces F_(g/b) ^(xyz), which is the barycenterof them. Due to its definition, p is only defined in the convex polygonrepresented by the contact points of the three wheels.

In this model, we consider directly the momentums between c and b(3)(4). This implies that we neglect the momentum induced by the arms.We can do that because in our cases of operation, there are not movingfast. Combining the equations (1)(2)(3)(4), we can write the dynamicequation of the system:

$\begin{matrix}{{{\overset{.}{L}}_{c} + {\overset{.}{L}}_{b}} = {{\left( {p - b} \right){m_{b}\left( {\overset{¨}{b} + g} \right)}} + {\left( {p - c} \right){m_{c}\left( {\overset{¨}{c} + g} \right)}}}} & (5)\end{matrix}$

We can note in the equation (5) that the dynamic equation of a two massmodel is simply the sum of two single ones.

Now, in order to linearize the equation (5), we formulate somehypotheses. Firstly, we can neglect the total angular momentum {dot over(L)}_(c)+{dot over (L)}_(b) because we have chosen to consider only themomentum between c and b. Secondly, because we have a redundant robot,we can move the CoM of c around the axes x and y without modifying itsposition on the axis z. So we consider c^(z) constrained at a constantvalue h; this hypothesis is important to obtain a linear model but therobot can still roll over its base without this hypothesis. Moreover tosimplify the description, we preferably consider a planar and horizontalground, so p^(z)=0. There is no DoF to control the position of b^(z), wecan set it to a constant I. Finally, we can note that g^(x)=g^(y)=0 andg^(z)=g the gravity norm.

Using these hypotheses and notes, we can rewrite the equation (5) asfollows:

m _(g) g(c ^(xy) −p ^(xy))+m _(b) g(b ^(xy) −p ^(xy))=m _(c) h{umlautover (c)} ^(xy) +m _(b) l{umlaut over (b)} ^(xy)   (6)

We can now use the equation (6) in order to provide a linear relationbetween the CoP and the positions and accelerations of the base and ofthe body:

$\begin{matrix}{p^{xy} = {\frac{{m_{c}c^{xy}} + {m_{b}b^{xy}}}{m_{c} + m_{b}} - \frac{{m_{c}h{\overset{¨}{c}}^{xy}} + {m_{b}l{\overset{¨}{b}}^{xy}}}{\left( {m_{c} + m_{b}} \right)g}}} & (17)\end{matrix}$

We can note that this model does not take into account any possibilityof tilting. So, to ensure the validity of the model, a constraint on theCoP must be added to the controller to ensure that the CoP is strictlyinside the convex polygon defined by the three wheels of the robot : arobustness criterion is defined. We consider

the convex polygon represented by the contact point of each wheel withthe ground; an example of this D polygon is shown FIG. 5. By definition,we always have pxY ∈

. To have a CoP constraint invariant by rotation, in the frame of therobot centered on b, we have designed a conservative constraint: p^(xy)∈

′, where z,54 is a circle centered on b of radius r, with the property

′ ∈

.

Quantifying the robustness of this system is an open question. In theabsence of any modelization of the disturbance forces, we cannot makeany hypothesis about the direction, the amplitude and their dynamics.The capacity of the robot to compensate a disturbance can be linked tothe CoP and the CoM position of c. As they are able to move, the robotcan react to a strong disturbance. We can note that, in the absence ofany hypothesis on the disturbance, the CoP and the CoM position of chave the most range to move in any direction if they are close to b. Sowe propose a robustness criteria (, which is equal to 0 on the maximumrobustness:

ζ=ζ_(f) ∥p−b∥+(1−ζ_(f))∥c−b∥  (8)

where ζ_(f) is a factor in the range [0; 1] which determines what typeof robustness we consider most important, to center the CoP, or tocenter the CoM of c.

This robot model being defined to address the case when the robot is nottilting, we can address the corresponding control law.

We can write the relation between each state, using the Euler explicitmethod:

$\begin{matrix}{c_{k + 1}^{xy} = {c_{k}^{xy} + {T\; {\overset{.}{c}}_{k}^{xy}} + {\frac{T^{2}}{2}{\overset{¨}{c}}_{k}^{xy}} + {\frac{T^{3}}{6}{\overset{...}{c}}_{k}^{xy}}}} & (9) \\{{\overset{.}{c}}_{k + 1}^{xy} = {{\overset{.}{c}}_{k}^{xy} + {T\; {\overset{¨}{c}}_{k}^{xy}} + {\frac{T^{2}}{2}{\overset{...}{c}}_{k}^{xy}}}} & (10) \\{{\overset{¨}{c}}_{k + 1}^{xy} = {{\overset{¨}{c}}_{k}^{xy} + {T{\overset{...}{c}}_{k}^{xy}}}} & (11)\end{matrix}$

There is an important consequence in the choice of the sampling periodT. The trajectory is allowed to be out of the constraints between twosamples, because we constrain the trajectory only at each sampling time.For real-time reason, we cannot choose a value forT too small. So it isnecessary to take into account this overflow in each constraint as asecurity margin.

Consider C^(xy)=(c₁ ^(xy). . . c_(N) ^(xy))^(t), and in the same way forthe derivatives of c and for b and p. Also, consider the initial stateĉ^(xy)=(c₀ ^(xy)ċ₀ ^(xy){umlaut over (c)}₀ ^(xy))^(t), and in the sameway for {circumflex over (b)}^(xy).

Using the equations (9)(10)(11), we can write the relation between eachderivative of the body trajectory (C, Ċ and {umlaut over (C)}) and thecommand {umlaut over (C)}:

C ^(xy) =U _(c)

+S _(c) ĉ ^(xy)   (12)

Ċ ^(xy) =U _(ċ)

+S _(ċ) ĉ ^(xy)   (13)

{umlaut over (C)} ^(xy) =U _({umlaut over (c)})

+S _({umlaut over (c)}) ĉ ^(xy)   (14)

With:

$\begin{matrix}{{U_{c} = \begin{pmatrix}\frac{T^{3}}{6} & 0 & 0 \\\vdots & \ddots & 0 \\\frac{\left( {1 - {3N} + {3N^{2}}} \right)T^{3}}{6} & \ldots & \frac{T^{3}}{6}\end{pmatrix}},{S_{c} = \begin{pmatrix}1 & T & \frac{T^{2}}{2} \\\vdots & \vdots & \vdots \\1 & {NT} & \frac{N^{2}T^{2}}{2}\end{pmatrix}}} & (15) \\{{U_{\overset{.}{c}} = \begin{pmatrix}\frac{T^{2}}{2} & 0 & 0 \\\vdots & \ddots & 0 \\\frac{\left( {{2N} - 1} \right)T^{2}}{2} & \ldots & \frac{T^{2}}{2}\end{pmatrix}},{S_{\overset{.}{c}} = \begin{pmatrix}0 & 1 & T \\\vdots & \vdots & \vdots \\0 & 1 & {NT}\end{pmatrix}}} & \; \\{{U_{\overset{¨}{c}} = \begin{pmatrix}T & 0 & 0 \\\vdots & \ddots & 0 \\T & \ldots & T\end{pmatrix}},{S_{\overset{¨}{c}} = \begin{pmatrix}0 & 0 & 1 \\\vdots & \vdots & \vdots \\0 & 0 & 1\end{pmatrix}}} & \;\end{matrix}$

The same way is used to define the dynamic of b. We can note that U_(c),U_(ċ) and U_({umlaut over (c)}) are invertible because they are squaredlower triangular matrices with no zero in the diagonal.

Concerning p, using the equation (6), we can write this relation:

P ^(xy) =U _(pc)

+U _(pb)

+S _(pc) ĉ ^(xy) +S _(pb) {circumflex over (b)} ^(xy)   (16)

With:

$\begin{matrix}{{{U_{pc} = \frac{m_{c}\left( {{gU}_{c} - {hU}_{\overset{¨}{c}}} \right)}{\left( {m_{c} + m_{b}} \right)g}},{U_{pb} = \frac{m_{b}\left( {{gU}_{b} - {lU}_{\overset{¨}{b}}} \right)}{\left( {m_{c} + m_{b}} \right)g}}}{{S_{pc} = \frac{m_{c}\left( {{gS}_{c} - {hS}_{\overset{¨}{c}}} \right)}{\left( {m_{c} + m_{b}} \right)g}},{S_{pb} = {\frac{m_{b}\left( {{gS}_{b} - {lS}_{\overset{¨}{b}}} \right)}{\left( {m_{c} + m_{b}} \right)g}.}}}} & (17)\end{matrix}$

Among different methods that can be used to solve the problem ofdetermining such a control law fulfilling these position, robustness anddynamic behavior conditions, we have chosen to address it as anoptimization problem. And to solve this optimization problem we havechosen to formulate it as a least square minimization under linearconstraints or as a quadratic optimization with objectives and linearconstraints. The main reason is that the solvers for this kind ofproblem are fast to compute. Adding non linear constraints or nonquadratic minimization objectives increase significantly thecomputational time.

The optimization variables correspond to the controlled body and baseX=(

)^(t). So each objective and constraint must be expressed as a functionof X.

1) Control Objectives:

The objectives O_(i) will be expressed as a least square minimizationand a QP (Quadratic Problem) formulation: O_(i)=½X^(t)Q_(i)X+p_(i)^(t)X.

X^(t) is X when transposed.

The first objective is the tracking control. In this control, we havechosen to do a position/velocity tracking. Let B_(ref) ^(xy) and {dotover (B)}_(ref) ^(xy) the position and velocity objective over thehorizon. Using (13), we can write the velocity control objective O₁:

$\begin{matrix}{O_{1} = {{\frac{1}{2}{{{\overset{.}{B}}^{xy} - {\overset{.}{B}}_{ref}^{xy}}}} = {{\frac{1}{2}X^{t}Q_{1}X} + {p_{1}^{t}X}}}} & (18) \\{{Q_{1} = \begin{pmatrix}0 & 0 & 0 & 0 \\0 & 0 & 0 & 0 \\0 & 0 & {U_{\overset{.}{b}}^{t}U_{\overset{.}{b}}} & 0 \\0 & 0 & 0 & {U_{\overset{.}{b}}^{t}U_{\overset{.}{b}}}\end{pmatrix}},{p_{1} = \begin{pmatrix}0 \\0 \\{U_{\overset{.}{b}}^{t}\left( {{S_{\overset{.}{b}}{\hat{b}}^{x}} - {\overset{.}{B}}_{ref}^{x}} \right)} \\{U_{\overset{.}{b}}^{t}\left( {{S_{\overset{.}{b}}{\hat{b}}^{y}} - {\overset{.}{B}}_{ref}^{y}} \right)}\end{pmatrix}}} & (19)\end{matrix}$

Using (12) we can write the position control objective O₂:

$\begin{matrix}{O_{2} = {{\frac{1}{2}{{B^{xy} - B_{ref}^{xy}}}} = {{\frac{1}{2}X^{t}Q_{2}X} + {p_{2}^{t}X}}}} & (20) \\{{Q_{2} = \begin{pmatrix}0 & 0 & 0 & 0 \\0 & 0 & 0 & 0 \\0 & 0 & {U_{b}^{t}U_{b}} & 0 \\0 & 0 & 0 & {U_{b}^{t}U_{b}}\end{pmatrix}},{p_{2}\begin{pmatrix}0 \\0 \\{U_{b}^{t}\left( {{S_{b}{\hat{b}}^{x}} - B_{ref}^{x}} \right)} \\{U_{b}^{t}\left( {{S_{b}{\hat{b}}^{y}} - B_{ref}^{y}} \right)}\end{pmatrix}}} & (21)\end{matrix}$

The next objective is the robustness maximization. By minimizing ζ wemaximize the robustness. Let U_(pbb)=U_(pb)−U_(b). Using (8), (12), (16)we can write the robustness control objective O₃:

$\begin{matrix}{O_{3} = {{\frac{1}{2}\zeta} \simeq {\frac{1}{2}{{{\zeta_{f}\left( {p - b} \right)} + {\left( {1 - \zeta_{f}} \right)\left( {c - b} \right)}}}}}} & (22) \\{O_{3} = {{\frac{1}{2}X^{t}Q_{3}X} + {p_{3}^{t}X}}} & (23) \\{Q_{3} = {{\zeta_{f}Q_{3p}} + {\left( {1 - \zeta_{f}} \right)Q_{3c}}}} & (24) \\{{p_{3} = {{\zeta_{f}p_{3p}} + {\left( {1 - \zeta_{f}} \right)p_{3c}}}}{Q_{3p} = \begin{pmatrix}{U_{pc}^{t}U_{pc}} & 0 & {U_{pc}^{t}U_{pbb}} & 0 \\0 & {U_{pc}^{t}U_{pc}} & 0 & {U_{pc}^{t}U_{pbb}} \\{U_{pbb}^{t}U_{pc}} & 0 & {U_{pbb}^{t}U_{pbb}} & 0 \\0 & {U_{pbb}^{t}U_{pc}} & 0 & {U_{pbb}^{t}U_{pbb}}\end{pmatrix}}{p_{3p} = \begin{pmatrix}{U_{pc}^{t}\left( {{\left( {S_{pb} - S_{b}} \right){\hat{b}}^{x}} + {S_{pc}{\hat{c}}^{x}}} \right)} \\{U_{pc}^{t}\left( {{\left( {S_{pb} - S_{b}} \right){\hat{b}}^{y}} + {S_{pc}{\hat{c}}^{y}}} \right)} \\{U_{pbb}^{t}\left( {{\left( {S_{pb} - S_{b}} \right){\hat{b}}^{x}} + {S_{pc}{\hat{c}}^{y}}} \right)} \\{U_{pbb}^{t}\left( {{\left( {S_{pb} - S_{b}} \right){\hat{b}}^{y}} + {S_{pc}{\hat{c}}^{y}}} \right)}\end{pmatrix}}} & (25) \\{Q_{3c} = \begin{pmatrix}{U_{c}^{t}U_{c}} & 0 & {{- U_{c}^{t}}U_{b}} & 0 \\0 & {U_{c}^{t}U_{c}} & 0 & {{- U_{c}^{t}}U_{b}} \\{{- U_{b}^{t}}U_{c}} & 0 & {U_{b}^{t}U_{b}} & 0 \\0 & {{- U_{b}^{t}}U_{c}} & 0 & {U_{b}^{t}U_{b}}\end{pmatrix}} & \; \\{p_{3c} = \begin{pmatrix}{U_{c}^{t}\left( {{S_{c}{\hat{c}}^{x}} - {S_{b}{\hat{b}}^{x}}} \right)} \\{U_{c}^{t}\left( {{S_{c}{\hat{c}}^{y}} - {S_{b}{\hat{b}}^{y}}} \right)} \\{U_{b}^{t}\left( {{S_{c}{\hat{c}}^{x}} - {S_{b}{\hat{b}}^{x}}} \right)} \\{U_{b}^{t}\left( {{S_{c}{\hat{c}}^{y}} - {S_{b}{\hat{b}}^{y}}} \right)}\end{pmatrix}} & \;\end{matrix}$

Preferably, we define another objective to avoid peak of jerk on therobot; so we add a jerk minimization objective O₄ to the control:

O ₄=½∥X∥=½X ^(t) Q ₄ X+p ₄ ^(t) X   (26)

Q ₄ =

,p ₄=0   (27)

Concerning this forth objective, we can note that when the robot falls,the trajectories of c and b grow exponentially. So by minimizing thejerk of these trajectories, we directly increase the avoidance ofexponential divergence of c and b, which contributes to stabilize therobot.

2) Control Constraints:

In the following the constraints R; will be expressed as a QP linearinequality constraint formulation: R_(i):v_(ī)≦V_(i)X≦v_(i) ⁺.

The first constraint R1 is to assure stability, to ensure that the robotwill not tilt by itself: ζ must be lower or equal to 1. This constraintis not linear, so we introduce a conservative set of linear constraintsto approximate this. We choose to use an octagonal shape of constraints

″ inscribed in the circle

′ as shown in FIG. 4: it defines a CoP limit constraint. Let

$\rho = {\frac{r}{\sqrt{10}}.}$

This constraint R1 which concerns the CoP bounds is written as follows:

$\begin{matrix}{{{R_{1}\text{:}\mspace{14mu} P^{xy}} - B^{xy}} \in ^{''}} & (28) \\{V_{1} = \begin{pmatrix}U_{d\; c} & 0 & {U_{pb} - U_{b}} & 0 \\0 & U_{pc} & 0 & {U_{pb} - U_{b}} \\U_{pc} & U_{pc} & {U_{pb} - U_{b}} & {U_{pb} - U_{b}} \\U_{pc} & {- U_{pc}} & {U_{pb} - U_{b}} & {{- U_{pb}} + U_{b}}\end{pmatrix}} & (29) \\{v_{1}^{-} = \begin{pmatrix}{{{- 3}\; \rho} - {S_{pc}{\hat{c}}^{x}} - {\left( {S_{pb} - S_{b}} \right){\hat{b}}^{x}}} \\{{{- 3}\; \rho} - {S_{pc}{\hat{c}}^{y}} - {\left( {S_{pb} - S_{b}} \right){\hat{b}}^{y}}} \\{{{- 4}\; \rho} - {S_{pc}\left( {{\hat{c}}^{x} + {\hat{c}}^{y}} \right)} - {\left( {S_{pb} - S_{b}} \right)\left( {{\hat{b}}^{x} + {\hat{b}}^{y}} \right)}} \\{{{- 4}\; \rho} - {S_{pc}\left( {{\hat{c}}^{x} - {\hat{c}}^{y}} \right)} - {\left( {S_{pb} - S_{b}} \right)\left( {{\hat{b}}^{x} - {\hat{b}}^{y}} \right)}}\end{pmatrix}} & \; \\{v_{1}^{+} = \begin{pmatrix}{{3\; \rho} - {S_{pc}{\hat{c}}^{x}} - {\left( {S_{pb} - S_{b}} \right)\hat{b}d^{x}}} \\{{3\; \rho} - {S_{pc}{\hat{c}}^{y}} - {\left( {S_{pb} - S_{b}} \right){\hat{b}}^{y}}} \\{{4\; \rho} - {S_{pc}\left( {{\hat{c}}^{x} + {\hat{c}}^{y}} \right)} - {\left( {S_{pb} - S_{b}} \right)\left( {{\hat{b}}^{x} + {\hat{b}}^{y}} \right)}} \\{{4\; \rho} - {S_{pc}\left( {{\hat{c}}^{x} - {\hat{c}}^{y}} \right)} - {\left( {S_{pb} - S_{b}} \right)\left( {{\hat{b}}^{x} - {\hat{b}}^{y}} \right)}}\end{pmatrix}} & \;\end{matrix}$

The second constraint R₂ concerns the kinematic limits of the mobilebase. Let {dot over (b)}_(max) and {umlaut over (b)}_(max) the maximumvelocity and acceleration of the mobile base. We can write theconstraint R₂ as follows:

$\begin{matrix}{R_{2}\text{:}\mspace{20mu} \left\{ \begin{matrix}{{- {\overset{.}{b}}_{\max}^{xy}} \leq {\overset{.}{B}}^{xy} \leq {\overset{.}{b}}_{\max}^{xy}} \\{{- {\overset{¨}{b}}_{\max}^{xy}} \leq {\overset{¨}{B}}^{xy} \leq {\overset{¨}{b}}_{\max}^{xy}}\end{matrix} \right.} & (30) \\{V_{2} = \begin{pmatrix}0 & 0 & U_{\overset{.}{b}} & 0 \\0 & 0 & 0 & U_{\overset{.}{b}} \\0 & 0 & U_{\overset{¨}{b}} & 0 \\0 & 0 & 0 & U_{\overset{¨}{b}}\end{pmatrix}} & (31) \\{{v_{2}^{-} = \begin{pmatrix}{{- {\overset{.}{b}}_{\max}} - {S_{\overset{.}{b}}{\hat{b}}^{x}}} \\{{- {\overset{.}{b}}_{\max}} - {S_{\overset{.}{b}}{\hat{b}}^{y}}} \\{{- {\overset{¨}{b}}_{\max}} - {S_{\overset{¨}{b}}{\hat{b}}^{x}}} \\{{- {\overset{¨}{b}}_{\max}} - {S_{\overset{¨}{b}}{\hat{b}}^{y}}}\end{pmatrix}},{v_{2}^{+} = \begin{pmatrix}{{\overset{.}{b}}_{\max} - {S_{\overset{.}{b}}{\hat{b}}^{x}}} \\{{\overset{.}{b}}_{\max} - {S_{\overset{.}{b}}{\hat{b}}^{y}}} \\{{\overset{¨}{b}}_{\max} - {S_{\overset{¨}{b}}{\hat{b}}^{x}}} \\{{\overset{¨}{b}}_{\max} - {S_{\overset{¨}{b}}{\hat{b}}^{y}}}\end{pmatrix}}} & \;\end{matrix}$

The third and last constraint R₃ concerns the kinematic limits of thebody. Due to its joints, the robot can move the CoM of its upper body ina rectangular zone

around the CoM of the mobile base. Let k^(xy) the limits of therectangular shape

, the constraint R₃ is written as follows:

$\begin{matrix}{{{R_{3}\text{:}}\mspace{14mu} - k^{xy}} \leq {c^{xy} - b^{xy}} \leq k^{xy}} & (32) \\{V_{3} = \begin{pmatrix}U_{c} & 0 & {- U_{b}} & 0 \\0 & U_{c} & 0 & {- U_{b}}\end{pmatrix}} & (33) \\{{v_{3}^{-} = \begin{pmatrix}{{- k^{x}} - {S_{c}{\hat{c}}^{x}} + {S_{b}{\hat{b}}^{x}}} \\{{- k^{y}} - {S_{c}{\hat{c}}^{y}} + {S_{b}{\hat{b}}^{y}}}\end{pmatrix}},{v_{3}^{+} = \begin{pmatrix}{k^{x} - {S_{c}{\hat{c}}^{x}} + {S_{b}{\hat{b}}^{x}}} \\{k^{y} - {S_{c}{\hat{c}}^{y}} + {S_{b}{\hat{b}}^{y}}}\end{pmatrix}}} & \;\end{matrix}$

The kinematic limits of the body can be null. Then we have:c^(xy)=b^(xy).

To solve this optimization problem with these objectives andconstraints, we use a linear quadratic problem solver. Some literatureabout solving a QP can be found in the book “Numerical optimization,second edition” of J. Nocedal and S. J. Wright 2000. This kind of solverfinds the optimal solution of a problem like this:

$\begin{matrix}\left\{ \begin{matrix}{\min\limits_{X}\left( {{X^{t}{QX}} + {p^{t}X}} \right)} \\{v^{-} \leq {VX} \leq v^{+}}\end{matrix} \right. & (34)\end{matrix}$

where Q is symmetric and positive definite. Using the equations (19),(21), (25), (27), (29), (31), (33), we can fill the values of Q, p, V,v⁻ and v⁺:

$\begin{matrix}{Q = {{\alpha_{1}Q_{1}} + {\alpha_{2}Q_{2}} + {\alpha_{3}Q_{3}} + {\alpha_{4}Q_{4}} + {\alpha_{5}Q_{5}}}} & (35) \\{p = {{\alpha_{1}p_{1}} + {\alpha_{2}p_{2}} + {\alpha_{3}p_{3}} + {\alpha_{4}p_{4}} + {\alpha_{5}p_{5}}}} & (36) \\{{V = \begin{pmatrix}V_{1} \\V_{2} \\V_{3}\end{pmatrix}},{v^{+} = \begin{pmatrix}v_{1}^{+} \\v_{2}^{+} \\v_{3}^{+}\end{pmatrix}},{v^{-} = \begin{pmatrix}v_{1}^{-} \\v_{2}^{-} \\v_{3}^{-}\end{pmatrix}}} & (37)\end{matrix}$

where α_(i) are the weightings associated to each objective. They can bechosen experimentally.

The choice of the α_(i) values is primordial to define the behavior ofthe control law. The relative gap between each α_(i) defines whichobjective will be prioritized, and which will be neglected. If α₁ and α₂are greater than the other weightings, the trajectory tracking will bevery efficient, but the robustness will be less efficient, and the jerkof the body and of the base can be high. If α₃ is greater than the otherweightings, the robot will be very robust to strong disturbances. We cannote that in this mode of behavior, if we define a positive velocity astracking objective, the robot will start backwards before movingforward, in order to center the CoP. The weighting α₄ has a smoothingeffect on the trajectories of c and b, if this weighting is greater thanthe other, the optimal solution will be not to move. Thereby, thisweighting must be small.

Some other behaviors can be obtained by choosing adequate weightings. Ifthe relative gap between two objectives is large (several orders ofmagnitude), the smaller objective will be computed almost in thenull-space of the larger one. Using this property is useful when we wantto have a pseudo constraint which can be relaxed if it cannot besatisfied. For example, we can add a high-weighted objective of the CoMof the upper body to center it to the mobile base position in order tohave a nice visual behavior. This will have the effect of fixing the CoMat the mobile base position whenever possible, but, in case this is notpossible, this pseudo-constraint will be relaxed.

The weighting set can be fixed, which means that we have chosen inadvance if we want to have a better trajectory tracking or robustness.

Referring to FIG. 2, the robot model and the control law are stored as abehavior 221; they are implemented by the execution engine 240 and bythe service module 210 according to a closed-loop scheme comprising thefollowing steps:

retrieving position measure of the body and position measure of thebase, from sensors,

converting these position measures in observed position measures, usingextractors 211,

calculating in a system service 213, body velocity and base velocitycommands using the previously described control law,

integrating these body and base velocities, to provide them to theextractors 211,

converting these commands (position and velocity for the body and forthe base) as instructions for the robot actuators 212: the wheels of thebase and joints of the robot.

The second controller is dedicated to the situation when the robot istilting. If the robot is about to tilt owing to a disturbance forexample, the model defined in the previous section is not sufficient. Weneed to take into account the tilt motion in the model as shown FIG. 4:tilt angle ψ is not null. First, we consider that the robot can onlytilt on two of its wheels. We do not consider the tilt only around onewheel because the robot looses too much DoF to be properly controlled inthat case.

When the robot is tilting, the CoP can only vary on the line between thetwo contact wheels. In order to minimize the probability to tilt on onewheel, the CoP is constrained to be in the middle of the two contactwheels. Moreover, the mechanical structure of the robot has been made toreduce the risks to tilt on one wheel. During the tilt motion, the robotlooses one DoF allowed by the in-air wheel. The result is that the robotbecomes non-holonomic. Some constraints in the controller must be addedto deal with that.

Consider v_(ψ) ^(xy) the unit vector on the xy-plane and orthogonal tothe tilt axis. p is only able to move on the axis defined by v_(ψ).Also, let ψ the tilt angle. Due to the ground, we have ψ≧0. We can splitc (and respectively b) into two components: a controlled part c_(c) (andrespectively b_(c)) by the motors of the robot, and another partdepending on the tilt angle by the following kinematic relations:

c ^(xy) =c _(c) ^(xy) +v _(ψ) ^(xy) d _(c) sin (ψ)   (38)

ċ ^(xy) =ċ _(c) ^(xy) +v _(ψ) ^(xy)(ċ _(c) sin (ψ)+d _(c){dot over (ψ)}cos (ψ))   (39)

{umlaut over (c)} ^(xy) ={umlaut over (c)} ^(xy) +v _(ψ) ^(xy)({umlautover (d)}_(c) sin (ψ)+2{dot over (d)} _(c){dot over (ψ)} cos (ψ) +d_(c){umlaut over (ψ)} cos (ψ)−d _(c){dot over (ψ)}² sin (ψ))  (40)

c ^(x) =h+d _(c) sin (ψ)   (41)

ċ ^(z) ={dot over (d)} _(c) sin (ψ)+d _(c){dot over (ψ)} cos (ψ)   (42)

{umlaut over (c)} ^(z) ={umlaut over (d)} _(c) sin (ψ)=2{dot over (d)}_(c){dot over (ψ)} cos (ψ)+d _(c){umlaut over (ψ)} cos (ψ)−d _(c){dotover (ψ)}ψ² sin (ψ)   (43)

with d_(c)=∥c_(c) ^(xy)−p^(xy)∥.In order to simplify these relations, we make two assumptions:

d_(c) can be considered constant because the motion of c_(c)^(xy)−p^(xy) relative to ψ is negligible during the transitional tiltmotion;

as usual, we neglect the Coriolis effects relative to the centrifugaleffects: {umlaut over (ψ)} cos (ψ)>>{dot over (ψ)} sin (ψ). So, theequations can be rewritten as follows:

c ^(xy) =c _(c) ^(xy) +v _(ψ) ^(xy) d _(c) sin (ψ)   (44)

ċ ^(xy) =ċ _(c) ^(xy) +v _(ψ) ^(xy) d _(c){dot over (ψ)} cos (104 )  (45)

{umlaut over (c)} ^(xy) ={umlaut over (c)} _(c) ^(xy) +v ₁₀₄ ^(xy) d_(c){umlaut over (ψ)} cos (ψ)   (46)

c ^(z) =h+d _(c) sin (ψ)   (47)

ċ ^(z) =d _(c){dot over (ψ)} cos (ψ)   (48)

{umlaut over (c)} ^(z) =d _(c){umlaut over (ψ)} cos (ψ)   (49)

Finally, we linearise these equations around ψ=0, considering that thetilt angle will stay close to 0. Also, we can consider that h>>d_(c) sin(ψ), when ψ≃0. The equations become:

c ^(xy) =c _(c) ^(xy) +v _(ψ) ^(xy) d _(c)ψ  (50)

ċ ^(xy) =ċ _(c) ^(xy) +v _(ψ) ^(xy) d _(c){dot over (ψ)}  (51)

{umlaut over (c)} ^(xy) ={umlaut over (c)} _(c) ^(xy) +v _(ψ) ^(xy) d_(c){umlaut over (ψ)}  (52)

c^(x)=h   (53)

ċ^(z)=0   (54)

{umlaut over (c)}^(z)=0   (55)

and in the same way for b, replacing d_(c) by d_(b) and h by I. Now,using (7)(50)(52)(53)(55), we can rewrite the equation of the dynamic asfollows:

$\begin{matrix}{p^{xy} = {\frac{{m_{c}\left( {c_{c}^{xy} + {v_{\psi}^{xy}d_{c}\psi}} \right)} + {m_{b}\left( {b_{c}^{xy} + {v_{\psi}^{xy}d_{b}\psi}} \right)}}{m_{c} + m_{b}} - \frac{{m_{c}{h\left( {{\overset{¨}{c}}_{c}^{xy} + {v_{\psi}^{xy}d_{c}\overset{¨}{\psi}}} \right)}} + {m_{b}{l\left( {{\overset{¨}{b}}_{c}^{xy} + {v_{\psi}^{xy}d_{b}\overset{¨}{\psi}}} \right)}}}{\left( {m_{c} + m_{b}} \right)g}}} & (56)\end{matrix}$

We notice that when the angular velocity is null (which means that therobot is in a no-tilt state), we obtain the equation (7) of the robotmodelization described in relation with the no-tilt controller.

We have chosen to constraint the position of p in the middle of the twocontact wheels. So, p is entirely defined by the following relation:

p=b _(c) +v _(ψ) d _(c)   (57)

Using (56)(57), we can now express the link between ψ and its derivativewith the other variables:

$\begin{matrix}{{{{\psi \left( {{m_{c}d_{c}} + {m_{b}d_{b}}} \right)}v_{\psi}} - {\overset{¨}{\psi}\frac{{m_{c}{hd}_{c}} + {m_{b}{ld}_{b}}}{g}v_{\psi}}} = {{m_{c}\left( {c_{c} - b_{c}} \right)} - \frac{{m_{c}h\; {\overset{¨}{c}}_{c}} + {m_{b}l\; {\overset{¨}{b}}_{c}}}{g} - {\left( {{m_{c} +}m_{c}} \right)d_{c}v_{\psi}}}} & (58)\end{matrix}$

This robot model with tilt being defined, we can address thecorresponding control law.

We use the same formalism as in the previous section. So we can definethe trajectories of ψ:

Ψ^(xy) =U _(ψ){umlaut over (Ψ)}^(xy) +S _(ψ){circumflex over (ψ)}^(xy)  (59)

{dot over (Ψ)}^(xy) =U _(ψ)

+S _(ψ){circumflex over (ψ)}^(xy)   (60)

{umlaut over (Ψ)}^(xy) =U _({umlaut over (ψ)})

+S _({umlaut over (ψ)}){circumflex over (ψ)}^(xy)   (61)

with Ψ^(xy)=(ψ₁ ^(xy). . . ψ_(N) ^(xy))^(t)and {umlaut over (ψ)}^(xy)=(ψ₀ ^(xy) {dot over (ψ)}₀ ^(xy) {umlaut over(ψ)}₀ ^(xy))^(t).

Using these relations and the equation (58), we have:

$\begin{matrix}{{{\left( {{U_{\psi}\overset{...}{\Psi}} + {S_{\psi}\hat{\psi}}} \right)\left( {{m_{c}d_{c}} + {m_{b}d_{b}}} \right)v_{\psi}} - {\left( {{U_{\overset{¨}{\psi}}\overset{...}{\Psi}} + {S_{\overset{¨}{\psi}}\hat{\psi}}} \right)\frac{{m_{c}{hd}_{c}} + {m_{b}{ld}_{b}}}{g}v_{\psi}}} = {{m_{c}\left( {C - B} \right)} - \frac{{m_{c}h\; \overset{¨}{C}} + {m_{b}l\; \overset{¨}{B}}}{g} - {\left( {m_{c} + m_{c}} \right)d_{c}v_{\psi}}}} & (62)\end{matrix}$

We can note that we have a direct linear relation between

and C,

, B and

using the equations (59) (61):

$\begin{matrix}{= {\left( {{{U_{\psi}\left( {{m_{c}d_{c}} + {m_{b}d_{b}}} \right)}v_{\psi}} - {U_{\overset{\sim}{\psi}}\frac{{m_{c}{hd}_{c}} + {m_{b}{ld}_{b}}}{g}v_{\psi}}} \right)^{- 1}\left( {{m_{c}\left( {C - B} \right)} - \frac{{m_{c}h\overset{¨}{C}} + {m_{b}l\overset{¨}{B}}}{g} - {\left( {m_{c} + m_{c}} \right)d_{c}v_{\psi}} - {S_{\psi}{\hat{\psi}\left( {{m_{c}d_{c}} + {m_{b}d_{b}}} \right)}v_{\psi}} + {S_{\overset{¨}{\psi}}\hat{\psi}\frac{{m_{c}{hd}_{c}} + {m_{b}{ld}_{b}}}{g}v_{\psi}}} \right)}} & (63)\end{matrix}$

And in a synthetic form:

=

+

+

+

  (64)

This relation is valid only if the matrix:

$\left( {{{U_{\psi}\left( {{m_{c}d_{c}} + {m_{b}d_{b}}} \right)}v_{\psi}} - {U_{\overset{¨}{\psi}}\frac{{m_{c}{hd}_{c}} + {m_{b}{ld}_{b}}}{g}v_{\psi}}} \right)$

is invertible. U_(ψ) and U_(ψ) are two lower triangular matrices withconstant diagonal of value

$\frac{T^{3}}{6}$

and T the sampling period. So, the invertibility is valid only if:

$\begin{matrix}{{{\frac{T^{3}}{6}\left( {{m_{c}d_{c}} + {m_{b}d_{b}}} \right)v_{\psi}} - {T\frac{{m_{c}{hd}_{c}} + {m_{b}{ld}_{b}}}{g}v_{\psi}}} \neq 0} & (65)\end{matrix}$

Solving this system, we have a condition on T to allow the invertibilityof this matrix (we assume that T>0):

$\begin{matrix}{T \neq \sqrt{\frac{{m_{c}{hd}_{c}} + {m_{b}{ld}_{b}}}{g\left( {{m_{c}d_{c}} + {m_{b}d_{b}}} \right)}}} & (66)\end{matrix}$

For the robot according to the invention with the features described inthe preamble in relation with FIG. 1 we have T≠220 ms.

To solve this problem we use the same formalism as in the previoussection (the robot is not tilting). The optimization variables remainthe same because despite adding the new variable ψ, we have constrainedthe CoP p to be at one point. So the number of variables to solve theproblem is unchanged:

X=(

)^(t)

1) Control Objectives:

The first objective is to minimize the norm of iv, in order to recoverthe tilt. We can express the relation between Ψ and X:

Ψ=U _(ψ)

+U _(ψ)

+U _(ψ)

ĉ+U ₁₀₄

{circumflex over (b)}+S _(ψ){circumflex over (ψ)}  (67)

Ψ=U _(ψc)

+U _(ψb)

+S _(ψc) ĉ+S _(ψb) {circumflex over (b)}+S _(ψ){circumflex over(ψ)}  (68)

With:

U _(ψc) =U _(ψ)

U _(ψb) =U _(ψ)

S _(ψc) =U _(ψ)

S _(ψb) =U _(ψ)

Now, we can write this objective O₁:

$\begin{matrix}{{O_{1} = {{\frac{1}{2}{\Psi }} = {{\frac{1}{2}X^{t}Q_{1}X} + {p_{1}^{t}X}}}}{Q_{1} = \begin{pmatrix}{U_{\psi \; c}^{t}U_{\psi \; c}} & 0 & {U_{\psi \; c}^{t}U_{\psi \; b}} & 0 \\0 & {U_{\psi \; c}^{t}U_{\psi \; c}} & 0 & {U_{\psi \; c}^{t}U_{\psi \; b}} \\{U_{\psi \; b}^{t}U_{\psi \; c}} & 0 & {U_{\psi \; b}^{t}U_{\psi \; b}} & 0 \\0 & {U_{\psi \; b}^{t}U_{\psi \; c}} & 0 & {U_{\psi \; b}^{t}U_{\psi \; b}}\end{pmatrix}}} & (70) \\{p_{1} = \begin{pmatrix}{U_{\psi \; c}^{t}\left( {{S_{\psi \; c}{\hat{c}}^{x}} + {S_{\psi \; b}{\hat{b}}^{x}} + {S_{\psi}\hat{\psi}}} \right)} \\{U_{\psi \; c}^{t}\left( {{S_{\psi \; c}{\hat{c}}^{y}} + {S_{\psi \; b}{\hat{b}}^{y}} + {S_{\psi}\hat{\psi}}} \right)} \\{U_{\psi \; b}^{t}\left( {{S_{\psi \; c}{\hat{c}}^{x}} + {S_{\psi \; b}{\hat{b}}^{x}} + {S_{\psi}\hat{\psi}}} \right)} \\{U_{\psi \; b}^{t}\left( {{S_{\psi \; c}{\hat{c}}^{y}} + {S_{\psi \; b}{\hat{b}}^{y}} + {S_{\psi}\hat{\psi}}} \right)}\end{pmatrix}} & (71)\end{matrix}$

The second objective is the tilt velocity minimization. In order to landwith the lowest angular velocity, to avoid rebounds as much as possibleand to minimize mechanical impacts, it is important to minimize theangular velocity during the tilt. We can express the relation between{dot over (ψ)} and X:

{dot over (Ψ)}=U _({dot over (ψ)})

+U _({dot over (ψ)})

+U _({dot over (ψ)})

ĉ+U _(ψ)

{circumflex over (b)}+S _({dot over (ψ)}){circumflex over (ψ)}   (72)

{dot over (Ψ)}=U _({dot over (ψ)}c)

+U _({dot over (ψ)}b)

+S _({dot over (ψ)}c) {umlaut over (c)}+S _({dot over (ψ)}b) {umlautover (b)}+S _({dot over (ψ)}){circumflex over (ψ)}  (73)

Now we can write this objective O₂:

$\begin{matrix}{{O_{2} = {{\frac{1}{2}{\Psi }} = {{\frac{1}{2}X^{t}Q_{2}X} + {p_{2}^{t}X}}}}{Q_{2} = \begin{pmatrix}{U_{\psi \; c}^{t}U_{\psi \; c}} & 0 & {U_{\psi \; c}^{t}U_{\psi \; b}} & 0 \\0 & {U_{\psi \; c}^{t}U_{\psi \; c}} & 0 & {U_{\psi \; c}^{t}U_{\psi \; b}} \\{U_{\psi \; b}^{t}U_{\psi \; c}} & 0 & {U_{\psi \; b}^{t}U_{\psi \; b}} & 0 \\0 & {U_{\psi \; b}^{t}U_{\psi \; c}} & 0 & {U_{\psi \; b}^{t}U_{\psi \; b}}\end{pmatrix}}} & (74) \\{p_{2} = \begin{pmatrix}{U_{\psi \; c}^{t}\left( {{S_{\psi \; c}{\hat{c}}^{x}} + {S_{\psi \; b}{\hat{b}}^{x}} + {S_{\psi}\hat{\psi}}} \right)} \\{U_{\psi \; c}^{t}\left( {{S_{\psi \; c}{\hat{c}}^{y}} + {S_{\psi \; b}{\hat{b}}^{y}} + {S_{\psi}\hat{\psi}}} \right)} \\{U_{\psi \; b}^{t}\left( {{S_{\psi \; c}{\hat{c}}^{x}} + {S_{\psi \; b}{\hat{b}}^{x}} + {S_{\psi}\hat{\psi}}} \right)} \\{U_{\psi \; b}^{t}\left( {{S_{\psi \; c}{\hat{c}}^{y}} + {S_{\psi \; b}{\hat{b}}^{y}} + {S_{\psi}\hat{\psi}}} \right)}\end{pmatrix}} & (75)\end{matrix}$

The third objective is for numerical stability, in a similar way withthe case without tilt:

O ₃+½∥X∥=½X ^(t) Q ₃ X+p ₃ ^(t) X   (76)

Q ₃ =

, p ₃=0   (77)

2) Control Constraints:

In the following, the constraints will be expressed as a QP linearequality constraint formulation: R_(i):v_(i) ^(−≦V) _(i)X≦v_(i) ^(+.)

The first constraint concerns the kinematic limits of the mobile base.Let {dot over (b)}_(max) and {umlaut over (b)}_(max) the maximumvelocity and acceleration of the mobile base, we can write theconstraint R₂ as follows:

$\begin{matrix}{R_{1}\text{:}\mspace{14mu} \left\{ {{\begin{matrix}{{- {\overset{.}{b}}_{\max}^{xy}} \leq {\overset{.}{B}}^{xy} \leq {\overset{.}{b}}_{\max}^{xy}} \\{{- {\overset{.}{b}}_{\max}^{xy}} \leq {\overset{¨}{B}}^{xy} \leq {\overset{¨}{b}}_{\max}^{xy}}\end{matrix}V_{1}} = \begin{pmatrix}0 & 0 & U_{\overset{.}{b}} & 0 \\0 & 0 & 0 & U_{\overset{.}{b}} \\0 & 0 & U_{\overset{¨}{b}} & 0 \\0 & 0 & 0 & U_{\overset{¨}{b}}\end{pmatrix}} \right.} & (78) \\{{v_{1}^{-} = \begin{pmatrix}{{- {\overset{.}{b}}_{\max}} - {S_{\overset{.}{b}}{\hat{b}}^{x}}} \\{{- {\overset{.}{b}}_{\max}} - {S_{\overset{.}{b}}{\hat{b}}^{y}}} \\{{- {\overset{¨}{b}}_{\max}} - {S_{\overset{¨}{b}}{\hat{b}}^{x}}} \\{{- {\overset{¨}{b}}_{\max}} - {S_{\overset{¨}{b}}{\hat{b}}^{y}}}\end{pmatrix}},{v_{1}^{+} = \begin{pmatrix}{{\overset{.}{b}}_{\max} - {S_{\overset{.}{b}}{\hat{b}}^{x}}} \\{{\overset{.}{b}}_{\max} - {S_{\overset{.}{b}}{\hat{b}}^{y}}} \\{{\overset{¨}{b}}_{\max} - {S_{\overset{¨}{b}}{\hat{b}}^{x}}} \\{{\overset{¨}{b}}_{\max} - {S_{\overset{¨}{b}}{\hat{b}}^{y}}}\end{pmatrix}}} & (79)\end{matrix}$

The second constraint concerns the kinematic limits of the upper body.Due to its joints, the robot can move the CoM of its upper body in arectangular zone around the CoM of the mobile base. Let k^(xy) thelimits of the rectangular shape

, the constraint R₂ is written as follows:

$\begin{matrix}{{{{R_{2}\text{:}}\mspace{14mu} - k^{xy}} \leq {c^{xy} - b^{xy}} \leq k^{xy}}{V_{2} = \begin{pmatrix}U_{c} & 0 & {- U_{b}} & 0 \\0 & U_{c} & 0 & {- U_{b}}\end{pmatrix}}} & (80) \\{{v_{2}^{-} = \begin{pmatrix}{{- k^{x}} - {S_{c}{\hat{c}}^{x}} + {S_{b}{\hat{b}}^{x}}} \\{{- k^{y}} - {S_{c}{\hat{c}}^{y}} + {S_{b}{\hat{b}}^{y}}}\end{pmatrix}},{v_{2}^{+} = \begin{pmatrix}{k^{x} - {S_{c}{\hat{c}}^{x}} + {S_{b}{\hat{b}}^{x}}} \\{k^{y} - {S_{c}{\hat{c}}^{y}} + {S_{b}{\hat{b}}^{y}}}\end{pmatrix}}} & (81)\end{matrix}$

The third constraint corresponds to the presence of the ground, whichimply that the tilt angle is always positive:

R₃:ψ≧0   (82)

V ₃=(U _({dot over (ψ)}c) U _({dot over (ψ)}c) U _({dot over (ψ)}b) U_({dot over (ψ)}b))v ₃ ⁻=(−S _({dot over (ψ)}c) ĉ ^(xy) −S_({dot over (ψ)}b) {circumflex over (b)} ^(xy) −S_({dot over (ψ)}){circumflex over (ψ)}), v ₃ ⁺=(+∞)   (83)

The last constraint means taking into account the DoF loss of the mobilebase. It is only possible to move b on the v_(|)axis. The constraint R₄is written as follows:

R ₄ :v _(ψ) ^(t) ×{dot over (b)} ^(xy)=0   (84)

V ₄=(−v _({dot over (ψ)}) ^(y) U _({dot over (b)}) v _({dot over (ψ)})^(x) U _({dot over (b)}) 0 0)v ₄ ⁻⁼⁽⁻ S _({dot over (b)})(v_({dot over (ψ)}) ^(x) −v _({dot over (ψ)}) ^(y)))   (85)

v ₄ ⁺=(−S _({dot over (b)})(v _({dot over (ψ)}) ^(x) −v_({dot over (ψ)}) ^(y)))   (86)

The third controller is dedicated to the landing phase. It is the sameas the first controller (no-tilt state), but without the two trackingcontrol objectives and with the constraint R₄ of the tilt controller.More precisely, the objectives are:

an objective related to the distance between the CoP and the basecenter, CoP being the barycenter of contact forces between the robot andthe ground, and

a numerical stability objective.

A base velocity objective can be added to these objectives.

The constraints are:

-   -   a maximum velocity and acceleration of the mobile base        kinematics limits of the body    -   a CoP limit    -   a move of the body only on the angular velocity axis.

In the lifetime of the robot, we have to choose which controller (shownin FIG. 6) to use:

-   -   The first one 501 to control the robot when it is not tilting.    -   The second one 502 used when the robot is tilting, in order to        recover the disturbance.

The third 503 one used when the robot is in landing phase.

According to these controllers we define three states for the robot:

-   -   The non-tilt state, when the first controller 501 is activated.    -   The tilt state, when the second controller 502 is activated.    -   The landing state, when the third controller 503 is activated.

We have developed an estimator 504 in order to predict the futurebehavior of the robot when it is tilting by estimating the landingimpact time and the impact angular velocity as described further.

As shown on FIG. 4, at each sampling time a supervisor 500 chooses oneof these three states according to the following steps.

Initially, the robot is in non-tilt state.

At each sampling time we measure the tilt angle and the tilt velocity:if one of them is greater than zero, we use the estimator to estimatethe impact angular velocity and the impact time.

A) When the robot is in non-tilt state.

-   -   If there is no estimated impact time (unstable motion), switch        to the tilt state, because the robot is not able to recover        without doing anything.    -   If the impact angular velocity is too high for the security of        the robot (=the impact angular velocity is above a pre-defined        limit1), switch to the tilt state, in order to reduce the impact        angular velocity.

If the measured angular velocity is too high (=the measured angularvelocity is above a pre-defined limit2), whatever the estimated impactvelocity, switch to the tilt state. This condition is intended to haveless delay to compensate a strong push. We assume that if a high angularvelocity is measured, this is because someone pushes the robot in orderto bring it down.

-   -   Else, there is no need to recover the angle, but if the tilt        angle is not null, switch to the landing state because the robot        has lost one DoF due to the in-air wheel.    -   Else if the tilt angle is null stay in this state.

B) When the robot is in tilt state.

In this state, the second controller is activated to minimize the tiltangle and tilt angular velocity of the robot. In this model, we do notmodelize the ground that will add a force to compensate the tilt motionat the impact. Controlling the angle at 0 makes the robot accelerationat a too high speed, in order to compensate the gravity acting on therobot at this angle. So we do not need to wait until the tilt isfinished to stop using this controller.

-   -   If the measured angular velocity is too high (=the measured        angular velocity is above a pre-defined limit2), whatever the        estimated impact velocity, stay in this state;    -   If the estimated impact velocity is behind the limit, switch to        landing state;    -   Else, stay in this state.

C) When the robot is in landing state.

This state manages a smooth transition between the tilt state and thenon-tilt state, firstly to avoid high base acceleration due to the nonmodelized ground in the tilt controller, and secondly in order to letthe in-air wheel fall and the robot recover the lost DoF beforereactivating the non-tilt state.

-   -   If there is no estimated impact time, or if the impact angular        velocity is too high, or if the measured angular velocity is too        high, switch in the tilt state;    -   If the tilt angle fall to 0, wait a little amount of time, in        case of a rebound appears, then if not, switch to the non-tilt        state;    -   Else, stay in this state.

We describe now the estimator. We have developed an estimator in orderto know if the robot will fall or not, and if not, which will be theimpact angular velocity. Using the current measured tilt angle {dot over(ψ)}₀ and angular velocity {dot over (ψ)}₀, we assume that we have aconstant acceleration

$\psi = {{- \frac{g}{d_{b}}}{{\cos \left( {\psi_{0}^{2} + {{atan}\left( \frac{l}{d_{b}} \right)}} \right)}.}}$

When the tilt angle {dot over (ψ)} is 0, the angle between the CoM ofthe mobile base and the tilt contact point is

${{atan}\left( \frac{l}{d_{b}} \right)}.$

This assumption means that the mobile base will stop to accelerate. Asthis estimator is used to know if we can use the landing mode (whichcontrol the acceleration of the mobile base to 0), considering aconstant angular acceleration is valid. We can solve the quadraticassociated system, in order to find the contact time t_(i) and if itexists, to compute the angular velocity at this moment {dot over(ψ)}_(i):

$\begin{matrix}{\overset{.}{\psi} = {{- \frac{{gt}\mspace{14mu} {\cos \left( {\psi_{0} + {{atan}\left( \frac{l}{d_{b}} \right)}} \right)}}{d_{b}}} + {\overset{.}{\psi}}_{0}}} & (87) \\{\psi = {{- \frac{{gt}^{2}{\cos \left( {\psi_{0} + {{atan}\left( \frac{l}{d_{b}} \right)}} \right)}}{2d_{b}}} + {{\overset{.}{\psi}}_{0}t} + \psi_{0}}} & (88)\end{matrix}$

If

${\Delta = {{{\overset{.}{\psi}}_{0}^{2} + \frac{2g\; \psi_{0}{\cos \left( {\psi_{0} + {{atan}\left( \frac{l}{d_{b}} \right)}} \right)}}{d_{b}}} < 0}},$

we have no impact. We can note that this can occur only if

$\psi_{0} > {\frac{\pi}{2} - {{{atan}\left( \frac{l}{d_{b}} \right)}.}}$

Else, the impact time t_(i) can be computed:

$\begin{matrix}{t_{i} = \frac{d_{b}\left( {{- {\overset{.}{\psi}}_{0}} \pm \sqrt{\Delta}} \right)}{g}} & (89)\end{matrix}$

Obviously, we choose the solution where t_(i)>0. And so, the estimatedimpact velocity {dot over (ψ)}_(i) is:

$\begin{matrix}{{\overset{.}{\psi}}_{i} = {{- \frac{{gt}_{i}}{d_{b}}} + {\overset{.}{\psi}}_{0}}} & (90)\end{matrix}$

The estimator and the supervisor can also be stored as a systemdata-service 213.

1. A humanoid robot with a body joined to an omnidirectional mobileground base, equipped with: a body position sensor, a base positionsensor and an angular velocity sensor to provide measures, actuatorscomprising joints motors and at least 3 wheels located in theomnidirectional mobile base, with at least one omnidirectional wheel,extractors for converting sensored measures, a supervisor to calculateposition, velocity and acceleration commands from the extracted data,means for converting commands into instructions for the actuators,wherein the supervisor comprises: a no-tilt state controller, a tiltstate controller and a landing state controller, each controllercomprising means for calculating from the extracted data, pre-orderedposition and velocity references, and a tilt angle and angular velocityreferences set to 0, position, velocity and acceleration commands basedon a double point-mass robot model and on a linear model predictivecontrol law with a discretized time according to a sampling time periodT and a number N of predicted samples, expressed as a quadraticoptimization formulation with a weighted sum of objectives, and a set ofpredefined linear constraints, an impact angular velocity and a landingimpact time estimator and means for choosing a controller according toan estimated impact angular velocity and extracted angular velocity. 2.The humanoid robot of claim 1, wherein the base has a constant angularacceleration.
 3. The humanoid robot of anyone of claim 1, wherein theno-tilt controller is based on a no-tilt robot model and is able tocalculate position, velocity and acceleration commands from theextracted data using pre-ordered references, and in that the objectivesare: a base position objective, a base velocity objective, an objectiverelated to the distance between the CoP and the base center, CoP beingthe barycenter of contact forces between the robot and the ground, andthe constraints are: a maximum velocity and acceleration of the mobilebase, a CoP limit.
 4. The humanoid robot of anyone of claim 1, whereinthe tilt controller is based on a robot model with tilt motion and isable to calculate position, velocity and acceleration commands from theextracted data using pre-ordered references and a tilt angle and angularvelocity references set to 0, and in that the objectives are tilt angleminimization and angular velocity minimization and the constraints arekinematic limits of the mobile base, kinematic limits of the body, apositive tilt angle and a move of the body only on the angular velocityaxis.
 5. The humanoid robot of claim 3, wherein a weighted numericalstability objective is added to the weighted sum of objectives.
 6. Thehumanoid robot of claim 1, wherein the landing controller is based on ano-tilt robot model and is able to calculate position, velocity andacceleration commands from the extracted data using pre-orderedreferences and a tilt angle and angular velocity references set to 0,and in that the objectives are an objective related to the distancebetween the CoP and the base center, CoP being the barycenter of contactforces between the robot and the ground, and a numerical stabilityobjective, the constraints are a maximum velocity and acceleration ofthe mobile base and kinematics limits of the body and a CoP limit and amove of the body only on the angular velocity axis.
 7. The humanoidrobot of claim 1, wherein a base velocity objective is added to theweighted sum of objectives.
 8. The humanoid robot of anyone of claim 3,wherein the constraints comprise kinematics limits of the body.
 9. Amethod for controlling a humanoid robot with a body joined to anomnidirectional mobile ground base, with actuators comprising at leastthree wheels with at least one omnidirectional wheel comprising:retrieving position measure of the body, position measure of the base,tilt angle of the robot and angular velocity measure of the robot, atpre-defined sampling times, converting these measures in extracted data,using the extracted data, and according to a defined tilt-state, orno-tilt state or landing state of the robot, state of the robot,calculating position, velocity and acceleration commands using atilt-state, or no-tilt state or landing state control law based on alinear model predictive control law with a discretized time according toa sampling time period and a number of predicted samples, and expressedas a quadratic optimization formulation with a weighted sum ofobjectives with predefined weights and a set of linear constraints,converting these commands into instructions for the robot actuators(212).
 10. The method of claim 9, wherein the state of the robot isdefined according to the following steps: if one among the tilt anglemeasure or and the angular velocity measure is greater than zero,estimate an estimated impact angular velocity and an estimated impacttime, initially, the robot is in a no-tilt state, No-tilt state: Ifthere is no estimated impact time, switch to a tilt state; If the impactangular velocity is above a pre-defined lim it1, switch to the tiltstate; If the measured angular velocity is above a pre-defined limit2,switch to the tilt state; Else, if the tilt angle is not null, switch toa landing state, if the tilt angle is null stay in the no-tilt state.Tilt state: If the measured angular velocity is above the pre-definedlimit2, stay in tilt state. If the estimated impact velocity is under apre-defined limit1, switch to the landing state Else, stay in tiltstate. Landing state: If there is no estimated impact time, switch tothe tilt state; If the impact angular velocity is above a pre-definedlimit1, switch to the tilt state; If the measured angular velocity isabove a pre-defined limit2, switch to the tilt state; If the tilt anglefalls to 0, and if an angular velocity measured at a next time is null,switch to the non-tilt state, else, stay in the landing state.
 11. Acomputer program linked to actuators of a humanoid robot comprisingcomputer code fit for executing when running on a computer the method ofclaim 9.